#ifndef _BUSINESS_H_
#define _BUSINESS_H_
#include "rads_database.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream> // 用于字符串流
#include <vector>  // 用于整数数组
#include <iostream>
#include <sqlite3.h>
#include <ctime>
#include <string>
#include <iomanip>
#include <cstdlib>
#include <sys/wait.h>
#include "ros/time.h" // 用于ros::Time
#include <tf/transform_listener.h>
#include <thread>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <fstream>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "rads/rads_scmdret.h"
#include "rads/rads_cmd.h"
#include "rads/rads_acks.h"
#include "rads/rads_version.h"
#include "rads/rads_versions.h"
#include "rads/rads_camera.h"
#include "rads/rads_log.h"
#include "rads/rads_cron.h"
#include "rads/rads_cronapi.h"
#include <chrono>
#include <mutex>
#include <cmath>
#include <stdexcept>
#include <functional>
#include <std_msgs/Float32.h>
#include "nav_msgs/Odometry.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include "serial/serial.h"
#include "HCNetSDK.h"
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"
using namespace std;
ros::Publisher sradsack_pub_;  // 发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）话题名《sradsack》
ros::Publisher sradsdrive;     // 发布控制车辆的信息,话题名《sradsdriveackermann》
ros::Publisher initialposepub; // 发布地图重定位,话题名《initialpose》
ros::Publisher pradscamerapub; // 发布双光谱摄像头报警信息
// ros::Publisher sradsversionpub;//发布OTA版本，话题名《sradsversion》
// ros::Publisher sradslogpub;// 发布日志话题，话题名《sradslog》
rads::rads_scmdret rads_scmdretdata;
// 全局变量和同步机制
using ThreadFunc_createline = std::function<void(int, int, string)>;  // 定义《带图带线路线点》传参
using ThreadFunc_createlineB = std::function<void(int, int, string)>; // 定义《带图带线路线点B》传参
using ThreadFunc_createline_all = std::function<void(int)>;           // 定义《马踏飞燕》传参
using ThreadFunc_createline_goback = std::function<void(int)>;        // 定义《跟推踩点》传参
using ThreadFunc_createline_goback_play = std::function<void(int)>;   // 定义《返航》传参

std::atomic<bool> should_run(false);             // 带图带线路线点
std::atomic<bool> should_runB(false);            // 带图带线路线点B
std::atomic<bool> should_run_all(false);         // 马踏飞燕
std::atomic<bool> should_run_goback(false);      // 跟推踩点
std::atomic<bool> should_run_goback_play(false); // 返航
std::atomic<bool> stopRadarThread(false);        // 自动驾驶
std::atomic<bool> stopRadarThread_R(false);      // 自动驾驶

std::thread thread_instance_createline;             // 定义《带图带线路线点》线程
std::thread thread_instance_createlineB;            // 定义《带图带线路线点》线程B
std::thread thread_instance_createline_all;         // 定义《马踏飞燕》线程
std::thread thread_instance_createline_goback;      // 定义《跟推踩点》线程
std::thread thread_instance_createline_goback_play; // 定义《返航》线程

ThreadFunc_createline thread_func_createline;                         // 带图带线路线点
ThreadFunc_createline thread_func_createlineB;                        // 带图带线路线点
ThreadFunc_createline_all thread_func_createline_all;                 // 马踏飞燕
ThreadFunc_createline_goback thread_func_createline_goback;           // 跟推踩点
ThreadFunc_createline_goback_play thread_func_createline_goback_play; // 返航

std::thread startinginit;

int Stop_lidar, stop_car, getmap_id;
int line_stop_flag = 0;
int key_1, key_2, key_3, key_4, key_5, key_6, key_7, key_8;
int ladar_1, ladar_2, ladar_3, ladar_4, ladar_5, ladar_6, ladar_7, ladar_8;
int Stop_lidar0, Stop_lidar1 = 0, Stop_lidar2 = 0, Stop_lidar3 = 0, Stop_lidar4 = 0, Stop_lidar5 = 0, Stop_lidar6 = 0, Stop_lidar7 = 0;
int FLLR = 0, LFRFLRRR = 0;
int heartbeat = 0;
int Release_radar = 0; // 1是解除雷达  0是 使用雷达
int modecaidian = 1;   //  1是双车道   0是单车道
double MAX_X, MIN_X, MAX_Y, MIN_Y, BMAX_Y, BMIN_Y, MIN_Z, HEADING_ERROR;
int STOP_3D_PONIT, BSTOP_3D_PONIT, RADAR_TEST;
static double last_latitude = 0.0;  // 双光谱摄像头报警信息
static double last_longitude = 0.0; // 双光谱摄像头报警信息
static ros::Time last_alarm_time;   // 双光谱摄像头报警信息
static bool is_first_alarm = true;  // 双光谱摄像头报警信息
std::atomic<bool> need_turnaround{true};
std::atomic<bool> goback_forward{true};   // 返航true=正向（向终点），false=反向（向起点）
std::atomic<int> current_target_index{0}; // 记录返航当前目标点索引
int cron_id = 0;                          // 当前总任务包的ID
int current_task_index;                   // 子任务顺序
// 新增：掉头状态枚举
enum TurnaroundState
{
  TS_READY,    // 准备掉头
  TS_TURNING,  // 正在掉头
  TS_COMPLETED // 掉头完成
};
TurnaroundState turnaround_state = TS_READY;
// rtk&&雷达所有数据
typedef struct
{
  int point_count;
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double yaw;
  double matching_rate;
  double longitude;
  double latitude;
  double altitude;
  double heading;
  double groud_speed;
  double posi_flag;
  double head_flag;
  double ext;
  double flag;
  double rtk_x;
  double rtk_y;
  double rtk_z;
  double f_x;
  double f_y;
  double f_z;
  double ld_heading;
  double base_length;
} locate_information;
locate_information lidar_rtk;
// 雷达返航使用的数据
typedef struct
{
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double ld_heading;
} lidar_goback;
lidar_goback lidar_gobackmsg;
// 外装的IMU
typedef struct
{
  double x;
  double y;
  double z;
} imu;
imu imu_msg;
// sradsack应答式发布
typedef struct
{
  string vehicle_id;
  string pro_nema;
  int cmd;
  int cmd_opt;
  int ext1;
  int ext2;
  int ack;
  int map_id;
  int path_id;
  string map_path;
  string progress;
} sradsack_fast;

// 摄像头配置结构体
struct CameraConfig
{
  std::string ip;
  std::string username;
  std::string password;
  std::string saveDir;
};

typedef struct
{
  int64_t recv_ts;        // 接收网络指令时系统的时间
  std::string nema;       //
  std::string head;       // ##
  std::string vehicle_id; // 设备编号
  int cmd;                // 命令项
  int cmd_opt;            // 命令选配扩展位2
  int ext;                // 扩展位1
  int ext2;               // 扩展位2
  std::vector<int> data;
} NetCmdObsolete;

pcb_log pcbmsg;
// 来自网络端的字符串数据解析
NetCmdObsolete DecodeNetCmdObsolete(std::string cloud_message)
{
  NetCmdObsolete cmd_obsolete;
  std::vector<std::string> vec = splitStr(cloud_message, ',');
  if (vec.size() > 4)
  {
    cmd_obsolete.recv_ts = current_timestamp_ms();
    cmd_obsolete.nema = cloud_message;
    cmd_obsolete.head = vec[0].substr(0, 2);
    cmd_obsolete.vehicle_id = vec[0].substr(2, vec[0].length() - 2);
    cmd_obsolete.cmd = atoi(vec[1].c_str());
    cmd_obsolete.cmd_opt = atoi(vec[2].c_str());
    cmd_obsolete.ext = atoi(vec[3].c_str());
    cmd_obsolete.ext2 = atoi(vec[4].c_str());
    // 针对 图,线,站点,分部调度
    if (CMDO_STATION_NAV == cmd_obsolete.cmd)
    {
      int len = cmd_obsolete.ext2;
      if (vec.size() > (len + 4))
      {
        for (int i = 0; i < len; i++)
        {
          cmd_obsolete.data.push_back(atoi(vec[i + 5].c_str()));
        }
      }
    }
    // 途经点导航
    else if (CMDO_PASSPOINT_NAV == cmd_obsolete.cmd)
    {
      int index = 5;
      int len = cmd_obsolete.ext;
      for (int i = 0; i < len * 2; i = i + 2)
      {
        cmd_obsolete.data.push_back(atoi(vec[index + i].c_str()));
        cmd_obsolete.data.push_back(atoi(vec[index + i + 1].c_str()));
      }
    }
    else if (CMDO_STATION_NAV_Z == cmd_obsolete.cmd)
    {
      int len = cmd_obsolete.ext2 * 2;
      if (vec.size() > (len + 4))
      {
        for (int i = 0; i < len; i++)
        {
          cmd_obsolete.data.push_back(atoi(vec[i + 5].c_str()));
        }
      }
    }
  }
  return cmd_obsolete;
}

void Cmd2radsmsg(NetCmdObsolete cmd, rads::rads_cmd &_cmd)
{
  _cmd.header.stamp = ros::Time::now();
  _cmd.header.frame_id = "nyd-net";
  _cmd.pro_nema = cmd.nema;
  _cmd.pro_head = cmd.head;
  _cmd.pro_vehsn = cmd.vehicle_id;
  _cmd.pro_cmd = cmd.cmd;
  _cmd.pro_cmd_opt = cmd.cmd_opt;
  _cmd.pro_ext1 = cmd.ext;
  _cmd.pro_ext2 = cmd.ext2;
  _cmd.data.clear();
  if (cmd.data.size() > 0)
  {
    for (int val : cmd.data)
    {
      _cmd.data.push_back(val);
    }
  }
}

void publishacks(ros::Publisher pub, const sradsack_fast &sradsack_fast_data)
{
  rads::rads_acks msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "sradsack_map"; // or whatever frame_id is appropriate
  msg.pro_nema = sradsack_fast_data.pro_nema;
  msg.cmd = sradsack_fast_data.cmd;
  msg.cmd_opt = sradsack_fast_data.cmd_opt;
  msg.ext1 = sradsack_fast_data.ext1;
  msg.ext2 = sradsack_fast_data.ext2;
  msg.ack = sradsack_fast_data.ack;
  msg.map_path = sradsack_fast_data.map_path;
  msg.map_id = sradsack_fast_data.map_id;
  msg.path_id = sradsack_fast_data.path_id;
  pub.publish(msg);
}
// 计算两个坐标点之间的距离
double distance_count(double aim_x, double aim_y, double pos_x, double pos_y)
{
  double distance = sqrt((aim_x - pos_x) * (aim_x - pos_x) + (aim_y - pos_y) * (aim_y - pos_y));
  return distance;
}

// 重定位
void Repositioning(int map_id)
{
  double minNDT_gl = 70.0; // 匹配度
  geometry_msgs::PoseWithCovarianceStamped pose_msg;
  pose_msg.header.stamp = ros::Time::now();
  pose_msg.header.frame_id = "map";
  pose_msg.pose.pose.position.x = 0;
  pose_msg.pose.pose.position.y = 0;
  pose_msg.pose.pose.position.z = 0;
  pose_msg.pose.pose.orientation.x = 0;
  pose_msg.pose.pose.orientation.y = 0;
  pose_msg.pose.pose.orientation.z = 0;
  pose_msg.pose.pose.orientation.w = 0;
  initialposepub.publish(pose_msg);
}
// 切换地图
bool map_ck(int map_id)
{

  char command[256];
  snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./map_loader.sh %d &", (int)map_id);
  int result = system(command);
  // Repositioning(map_id);
  return result;
}

/*任意点导航 start*/
// 计算两个坐标点之间的距离
double DistanceCount(AxisData &data1, AxisData &data2)
{
  double distance = sqrt(pow((data1.x - data2.x), 2) + pow((data1.y - data2.y), 2));
  return distance;
}
// 创建节点 value点的索引值
ListNode *CreateNode(int value)
{
  return new ListNode(value);
}
// 向前插入节点
void InsertNodeToHead(ListNode *&head, int value)
{
  ListNode *newNode = CreateNode(value);
  newNode->next = head;
  head = newNode;
}
// 打印链表
void PrintList(ListNode *head)
{
  Path_test[0] = 0;
  while (head != nullptr)
  {
    std::cout << head->value << " -> ";
    Path_test[0]++;
    Path_test[Path_test[0]] = head->value;
    head = head->next;
  }
  std::cout << "null" << std::endl;
}
// 输入一个链表，返回int数组，点位总数（第一位）和点位索引（从索引1开始往后表示）
int *CopyList(ListNode *head)
{
  static int copy_data[1000]; // 需要返回的值和点位
  copy_data[0] = 0;
  while (head != nullptr)
  {
    copy_data[0]++;
    copy_data[copy_data[0]] = head->value;
    head = head->next;
  }
  return copy_data;
}
// 输入坐标x，y 匹配最近点的索引
int FindPoint(AxisData &data, std::vector<point_all> &points)
{
  double minDistance = std::numeric_limits<double>::max(); // 初始化一个最大距离用于计算邻近点比较器
  for (auto i = 0; i != points.size(); i++)
  {
    AxisData tmp;
    tmp.x = points[i].x;
    tmp.y = points[i].y;
    double distance = DistanceCount(data, tmp);
    if (distance < minDistance)
    {
      minDistance = distance, Current_NUM = i;
    }
  }
  // path_pub.publish(path);
  return Current_NUM;
}
// 找当前索引点最近距离点
int *FindPointNum(int point_num, std::vector<point_all> &points)
{
  static int point_data[5]; // 需要返回的值和点位
  point_data[0] = 0;
  for (auto i = 0; i != points.size(); i++)
  {

    AxisData aim(Nav_x[point_num], Nav_y[point_num]);
    AxisData pos(Nav_x[i], Nav_y[i]);
    double distance = DistanceCount(aim, pos);
    if (distance < 3.5 && distance > 1.5)
    {
      point_data[0]++;
      point_data[point_data[0]] = i;
    }
  }
  if (point_data[0] > 0)
  {
    Nav_x[point_num] = 99999;
    Nav_y[point_num] = 99999;
  }
  return point_data;
}
// 计算起点和目标点的规划路径
vector<int16_t> PathPlaning(AxisData &curPose, AxisData &aimPose, std::vector<point_all> &points)
{
  int jump_flag = 0;
  int Line_num = 0;
  int Line_count = 1;
  vector<int16_t> res;
  // 起点 给出坐标值，输出该坐标的标号//当前坐标
  int pos_num = FindPoint(curPose, points);
  // 目标点 给出坐标值，输出该坐标的标号//目标
  int aim_num = FindPoint(aimPose, points);

  if (pos_num == aim_num)
  {

    res.push_back(aim_num);
  }
  else
  {

    for (int i = 0; i < 180; ++i)
    {
      lists[i] = nullptr;
    }
    InsertNodeToHead(lists[0], aim_num); // 把30条路线的头插入 第一个点，把初始搜索点插入到第0条线路的第一个
    int count20 = 0;
    //********************         导航算法如下           *************************
    for (auto i = 0; i != points.size(); i++)
    {
      if (jump_flag == 1)
      {
        break;
      }
      int Line_data = Line_count;
      for (int j = 0; j < Line_data; ++j) // 有多少根线，对每根线操作   J 代表第几根线
      {
        count20++;
        if (jump_flag == 1)
        {
          break;
        }

        int *p = FindPointNum(lists[j]->value, points); // 把每个线路的最后一个点位查找附近点数量//    如果复制链表，需要在此处下复制
        if (*(p) == 1)
        {
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
        }
        else if (*(p) == 2)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 1;
        }
        else if (*(p) == 3)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 2;
        }
        else if (*(p) == 4)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 2], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 2], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 3;
        }

        for (size_t x = 0; x < *p; x++)
        {
          if (*(p + 1 + x) == pos_num)
          {
            jump_flag = 1;
            Line_num = j;
            break;
          }
        }
      }
    }
    if (Line_num >= 0)
    {
      ListNode *head = lists[Line_num];
      // PrintList(head);
      while (head != nullptr)
      {
        res.push_back(head->value);
        head = head->next;
      }
    }
  }

  return res;
}
std::vector<int16_t> getPath_anypoint(double x1, double y1, double x2, double y2, std::vector<point_all> &points)
{
  AxisData currentPosition(x1, y1);
  AxisData targetPosition(x2, y2);
  return PathPlaning(currentPosition, targetPosition, points);
}
/*任意点导航 end*/

// 角度归一化函数
double normalize_angle(double angle)
{
  while (angle < -M_PI)
    angle += 2 * M_PI;
  while (angle >= M_PI)
    angle -= 2 * M_PI;
  return angle;
}

// 字符串分割函数
std::vector<std::string> splitStr_cmd(const std::string &str, char delimiter)
{
  std::vector<std::string> tokens;
  std::string token;
  std::istringstream tokenStream(str);
  while (std::getline(tokenStream, token, delimiter))
  {
    tokens.push_back(token);
  }
  return tokens;
}

// 指令合并方法
std::string mergeInstructions(const std::string &input)
{
  // 检测是否包含两个##
  size_t firstHashPos = input.find("##");
  size_t secondHashPos = input.find("##", firstHashPos + 2); // 从第一个##后开始查找第二个##

  // 如果找到两个##，则视为需要合并的指令
  if (firstHashPos != std::string::npos && secondHashPos != std::string::npos &&
      secondHashPos > firstHashPos)
  {
    std::cout << "检测到需要合并的指令..." << std::endl;

    // 分割成两条原始指令
    std::string cmd1 = input.substr(0, secondHashPos);
    std::string cmd2 = input.substr(secondHashPos);

    // 解析第一条指令（29指令）
    std::vector<std::string> parts1 = splitStr_cmd(cmd1, ',');
    if (parts1.size() < 6)
    {
      std::cout << "错误：第一条指令格式不正确" << std::endl;
      return input; // 返回原始输入
    }

    // 解析第二条指令（291指令）
    std::vector<std::string> parts2 = splitStr_cmd(cmd2, ',');
    if (parts2.size() < 6)
    {
      std::cout << "错误：第二条指令格式不正确" << std::endl;
      return input; // 返回原始输入
    }

    // 提取公共部分
    std::string head = parts1[0];            // ##设备编号
    int mapId = std::stoi(parts1[2]);        // 地图编号
    int lineId = std::stoi(parts1[3]);       // 路线编号
    int stationCount = std::stoi(parts1[4]); // 站点数量

    // 提取站点编号（来自29指令）
    std::vector<std::string> stationIds;
    for (int i = 5; i < 5 + stationCount && i < parts1.size(); i++)
    {
      stationIds.push_back(parts1[i]);
    }

    // 提取站点描述（来自291指令）
    std::vector<std::string> stationDescriptions;
    for (int i = 5; i < 5 + stationCount && i < parts2.size(); i++)
    {
      stationDescriptions.push_back(parts2[i]);
    }

    // 验证站点数量是否匹配
    if (stationIds.size() != stationCount || stationDescriptions.size() != stationCount)
    {
      std::cout << "错误：站点数量不匹配" << std::endl;
      return input; // 返回原始输入
    }

    // 构造合并后的指令
    std::string mergedCmd = head + ",299," + std::to_string(mapId) + "," +
                            std::to_string(lineId) + "," + std::to_string(stationCount);

    // 添加站点编号
    for (const auto &id : stationIds)
    {
      mergedCmd += "," + id;
    }

    // 添加站点描述
    for (const auto &desc : stationDescriptions)
    {
      mergedCmd += "," + desc;
    }

    std::cout << "合并后的指令: " << mergedCmd << std::endl;
    return mergedCmd;
  }
  else
  {
    // 只有一个##，直接返回原始指令
    std::cout << "单个指令，直接返回: " << input << std::endl;
    return input;
  }
}

int findNearestToFirst(const std::vector<int> &nearest_points, int target)
{
  // 数组为空时返回-1
  if (nearest_points.empty())
  {
    std::cout << "数组为空，无可用元素" << std::endl;
    return -1;
  }

  // 初始化结果为数组第一个元素，最小差值为两者的绝对值
  int best_match = nearest_points[0];
  int min_diff = std::abs(best_match - target);

  // 遍历数组寻找与目标索引差值最小的元素
  for (int point : nearest_points)
  {
    int current_diff = std::abs(point - target);

    // 找到更小的差值，更新最佳匹配
    if (current_diff < min_diff)
    {
      min_diff = current_diff;
      best_match = point;
    }
    // 差值相同的情况下，优先选择较小的点（可根据实际需求调整）
    else if (current_diff == min_diff && point < best_match)
    {
      best_match = point;
    }
  }

  // 输出结果
  std::cout << "目标索引: " << target
            << ", 最佳匹配: " << best_match
            << ", 路径差值: " << min_diff << std::endl;

  return best_match;
}

// 吸附点返回 num_point；
std::vector<int> findcronpoint(const std::vector<car> &pathPoints)
{
  std::vector<int> nearest_indices;
  const double MAX_SEARCH_DISTANCE = 8.0;              // 距离阈值
  std::vector<std::pair<double, int>> point_distances; // 存储(距离, 点编号)

  std::cout << "\n=== 开始查找最近的点 ===" << std::endl;
  std::cout << "搜索距离阈值: " << MAX_SEARCH_DISTANCE << "m" << std::endl;

  // 遍历所有点计算距离
  for (size_t i = 0; i < pathPoints.size(); ++i)
  {
    const car &point = pathPoints[i];
    double distance;

    // 根据匹配率选择坐标系计算距离
    if (lidar_rtk.matching_rate > 45.0)
    {
      distance = distance_count(lidar_rtk.x, lidar_rtk.y, point.x, point.y);
    }
    else
    {
      distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, point.rtk_x, point.rtk_y);
    }

    // 打印所有点的距离信息
    std::cout << "点索引: " << point.num_point
              << ", 距离: " << distance << "m";

    // 只考虑在距离阈值范围内的点
    if (distance <= MAX_SEARCH_DISTANCE)
    {
      point_distances.emplace_back(distance, point.num_point);
      std::cout << " -> 符合距离条件，加入候选" << std::endl;
    }
    else
    {
      std::cout << " -> 距离超出阈值，跳过" << std::endl;
    }
  }

  // 按距离升序排序
  std::sort(point_distances.begin(), point_distances.end(),
            [](const std::pair<double, int> &a, const std::pair<double, int> &b)
            {
              return a.first < b.first;
            });

  // 取前5个最近的点
  size_t count = std::min(static_cast<size_t>(5), point_distances.size());
  std::cout << "\n--- 筛选结果 ---" << std::endl;
  std::cout << "符合条件的候选点数量: " << point_distances.size() << std::endl;
  std::cout << "选取最近的" << count << "个点:" << std::endl;

  for (size_t i = 0; i < count; ++i)
  {
    nearest_indices.push_back(point_distances[i].second);
    std::cout << "排名" << (i + 1) << ": 点索引=" << point_distances[i].second
              << ", 距离=" << point_distances[i].first << "m" << std::endl;
  }

  std::cout << "========================\n"
            << std::endl;
  return nearest_indices;
}

// 新增辅助函数：弧度归一化到 [-PI, PI]
double normalize_angle_rad(double angle)
{
  while (angle > M_PI)
    angle -= 2 * M_PI;
  while (angle < -M_PI)
    angle += 2 * M_PI;
  return angle;
}

// 计算两点连线的方向角（度）
double calculate_direction(double x1, double y1, double x2, double y2)
{
  double dx = x2 - x1;
  double dy = y2 - y1;
  double rad = std::atan2(dy, dx);
  return normalize_angle(rad * 180.0 / M_PI);
}

// 激光雷达
double calculate_NDT_JDC(const car &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.x - target_point.x;
  delta_y = lidar_rtk.y - target_point.y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + HEADING_ERROR; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.ld_heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}
// rtk
double rtkcalculate_NDT_JDC(const car &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.rtk_x - target_point.rtk_x;
  delta_y = lidar_rtk.rtk_y - target_point.rtk_y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + 270; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}

int findSnappedWaypoint(const std::vector<car> &pathPoints)
{
  // 核心参数配置
  const int NEARBY_POINTS_COUNT = 8;       // 筛选最近的6个点
  const double MAX_ABS_ANGLE = 90.0;       // 角度绝对值阈值（<90°视为有效）
  const double MAX_SEARCH_DISTANCE = 10.0; // 最大搜索距离（米）
  // 调试信息：算法开始
  std::cout << "\n======================================" << std::endl;
  std::cout << "========== 吸附点搜索算法启动 =========" << std::endl;
  std::cout << "======================================" << std::endl;
  std::cout << "系统状态: " << std::endl;
  std::cout << "  - 匹配率: " << lidar_rtk.matching_rate << "%" << std::endl;
  std::cout << "  - 车辆位置: "
            << "X=" << (lidar_rtk.matching_rate > 45.0 ? lidar_rtk.x : lidar_rtk.rtk_x) << ", "
            << "Y=" << (lidar_rtk.matching_rate > 45.0 ? lidar_rtk.y : lidar_rtk.rtk_y) << std::endl;
  std::cout << "  - 车辆航向: "
            << (lidar_rtk.matching_rate > 45.0 ? lidar_rtk.ld_heading : lidar_rtk.heading) << "°" << std::endl;
  std::cout << "  - 路径点总数: " << pathPoints.size() << std::endl;
  std::cout << "  - 筛选参数: 最近" << NEARBY_POINTS_COUNT << "个点，最大距离"
            << MAX_SEARCH_DISTANCE << "m，角度绝对值<" << MAX_ABS_ANGLE << "°" << std::endl;

  // 路径点为空检查
  if (pathPoints.empty())
  {
    std::cout << "[ERROR] 路径点列表为空，无法执行吸附！" << std::endl;
    return -1;
  }

  // 步骤1：筛选所有在搜索范围内的点并按距离排序
  std::vector<std::pair<int, double>> nearby_points; // 存储<点索引, 距离>
  for (size_t i = 0; i < pathPoints.size(); ++i)
  {
    // 获取路径点坐标（根据匹配率选择坐标系）
    const car &point = pathPoints[i];
    double point_x = (lidar_rtk.matching_rate > 45.0) ? point.x : point.rtk_x;
    double point_y = (lidar_rtk.matching_rate > 45.0) ? point.y : point.rtk_y;

    // 计算车辆到该点的直线距离
    double distance = distance_count(
        (lidar_rtk.matching_rate > 45.0 ? lidar_rtk.x : lidar_rtk.rtk_x),
        (lidar_rtk.matching_rate > 45.0 ? lidar_rtk.y : lidar_rtk.rtk_y),
        point_x, point_y);

    // 仅保留距离在阈值内的点
    if (distance <= MAX_SEARCH_DISTANCE)
    {
      nearby_points.emplace_back(i, distance);
      std::cout << "[DEBUG] 点 " << i << " 距离=" << distance << "m（在搜索范围内）" << std::endl;
    }
  }

  // 按距离升序排序，取最近的6个点
  std::sort(nearby_points.begin(), nearby_points.end(),
            [](const std::pair<int, double> &a, const std::pair<int, double> &b)
            {
              return a.second < b.second;
            });

  // 截取前6个点（若不足6个则保留全部）
  if (nearby_points.size() > NEARBY_POINTS_COUNT)
  {
    nearby_points.resize(NEARBY_POINTS_COUNT);
  }

  // 检查是否有可用的候选点
  if (nearby_points.empty())
  {
    std::cout << "[ERROR] 未找到距离≤" << MAX_SEARCH_DISTANCE << "m的点！" << std::endl;
    return -1;
  }

  // 调试信息：显示筛选出的最近6个点
  std::cout << "\n[INFO] 筛选出的最近" << nearby_points.size() << "个点：" << std::endl;
  for (size_t i = 0; i < nearby_points.size(); ++i)
  {
    std::cout << "  排名" << i + 1 << ": 点" << nearby_points[i].first
              << "（距离=" << nearby_points[i].second << "m）" << std::endl;
  }

  // 步骤2：检查每个候选点及其后续点的角度绝对值是否小于90°
  std::vector<int> valid_candidates;    // 存储角度有效的点
  std::vector<double> candidate_scores; // 存储有效点的评分（距离越小评分越低）

  for (const auto &np : nearby_points)
  {
    int base_idx = np.first;          // 当前候选点索引
    double base_distance = np.second; // 当前候选点距离

    std::cout << "\n[INFO] 开始检查点" << base_idx << "的角度有效性：" << std::endl;

    // 检查当前点及后续2个点的角度绝对值（共3个点）
    std::vector<std::pair<int, double>> angle_sequence; // <点索引, 角度值>
    bool all_valid = true;                              // 标记所有点是否都满足角度条件
    for (int step = 0; step < 3; ++step)
    {
      int target_idx = base_idx + step;

      // 检查索引是否超出路径范围
      if (target_idx >= (int)pathPoints.size())
      {
        std::cout << "[DEBUG] 点" << target_idx << "超出路径范围（最大索引"
                  << pathPoints.size() - 1 << "），停止后续检查" << std::endl;
        break;
      }

      // 获取该点的计算角度（原始值）
      const car &target_point = pathPoints[target_idx];
      double exec_angle = (lidar_rtk.matching_rate > 45.0)
                              ? calculate_NDT_JDC(target_point)
                              : rtkcalculate_NDT_JDC(target_point);

      angle_sequence.emplace_back(target_idx, exec_angle);
      std::cout << "[DEBUG] 点" << target_idx << "的计算角度：" << exec_angle << "°，绝对值：" << fabs(exec_angle) << "°" << std::endl;

      // 检查角度绝对值是否小于90°
      if (fabs(exec_angle) >= MAX_ABS_ANGLE)
      {
        all_valid = false;
        std::cout << "[DEBUG] 点" << target_idx << "角度绝对值=" << fabs(exec_angle) << "°（≥阈值" << MAX_ABS_ANGLE << "°）" << std::endl;
      }
    }

    // 处理路径末尾点（不足3个点的情况）
    if (angle_sequence.size() < 2)
    {
      std::cout << "[WARNING] 点" << base_idx << "后续点不足，默认视为有效" << std::endl;
      valid_candidates.push_back(base_idx);
      candidate_scores.push_back(base_distance);
      continue;
    }

    // 判断是否所有点的角度绝对值都有效
    if (all_valid)
    {
      std::cout << "[INFO] 点" << base_idx << "及其后续点角度绝对值均<" << MAX_ABS_ANGLE << "°（有效候选点）" << std::endl;
      valid_candidates.push_back(base_idx);
      candidate_scores.push_back(base_distance);
    }
    else
    {
      std::cout << "[INFO] 点" << base_idx << "或其后续点角度绝对值超标（无效）" << std::endl;
    }
  }

  // 步骤3：从有效候选点中选择最优吸附点
  int snapped_index = -1;
  if (!valid_candidates.empty())
  {
    // 找到距离最近的有效点
    size_t best_idx = 0;
    double min_score = candidate_scores[0];
    for (size_t i = 1; i < candidate_scores.size(); ++i)
    {
      if (candidate_scores[i] < min_score)
      {
        min_score = candidate_scores[i];
        best_idx = i;
      }
    }
    snapped_index = valid_candidates[best_idx];
    std::cout << "\n[INFO] 最优吸附点：点" << snapped_index
              << "（距离=" << min_score << "m，角度均有效）" << std::endl;
  }
  else
  {
    // 兜底逻辑：若无有效点，选择最近的点
    snapped_index = nearby_points[0].first;
    std::cout << "\n[WARNING] 无角度有效的点，选择最近点：点" << snapped_index
              << "（距离=" << nearby_points[0].second << "m）" << std::endl;
  }

  // RADS_R模式处理
  // if (RADS_R == 1)
  // {
    if (snapped_index >= static_cast<int>(pathPoints.size()) - 1)
    {
      snapped_index = 0;
      std::cout << "RADS_R模式: 重置为第一个路径点" << std::endl;
    }
  //}

  // 调试信息：算法结束
  std::cout << "\n======================================" << std::endl;
  std::cout << "最终吸附点索引：" << snapped_index << std::endl;
  std::cout << "======================================\n"
            << std::endl;

  return snapped_index;
}

// goback激光雷达
double goback_calculate_NDT_JDC(const go_back &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_gobackmsg.x - target_point.x;
  delta_y = lidar_gobackmsg.y - target_point.y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + HEADING_ERROR; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_gobackmsg.ld_heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}

// rtk
double goback_rtkcalculate_NDT_JDC(const go_back &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.rtk_x - target_point.rtk_x;
  delta_y = lidar_rtk.rtk_y - target_point.rtk_y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + 270; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}
// 定义常量PI
const double PIS = 3.14159265358979323846;
// 将角度转换为弧度
double degreesToRadians(double degrees)
{
  return degrees * PIS / 180.0;
}
// 计算两个天线中间位置的坐标
void calculateMidPoint(double x1, double y1, double yawDegrees, double baseline, double &xMid, double &yMid)
{
  // 将航向角度转换为弧度
  double yawRadians = degreesToRadians(yawDegrees);
  // 计算第二个天线的坐标
  double deltaX = cos(yawRadians) * baseline;
  double deltaY = sin(yawRadians) * baseline;
  double x2 = x1 + deltaX;
  double y2 = y1 + deltaY;
  // 计算中间位置的坐标
  xMid = (x1 + x2) / 2.0;
  yMid = (y1 + y2) / 2.0;
}

void pubdrive(int brake, int gear, int steering, int target_speed)
{
  heartbeat++;
  rads_scmdretdata.heartbeat = heartbeat;
  rads_scmdretdata.len = 25;      // len
  rads_scmdretdata.brake = brake; // 制动值0-100
  rads_scmdretdata.gear = gear;   // 0=P，1=N，2=D，3=R，4=O；
  rads_scmdretdata.steering = steering;
  rads_scmdretdata.target_speed = target_speed; // 速度：单位：KM/h
  rads_scmdretdata.header.stamp = ros::Time::now();
  rads_scmdretdata.header.frame_id = "sradsdriveackermann";
  sradsdrive.publish(rads_scmdretdata);
}
// 计算当前车辆剩余里程，Lidar_msg当前车辆的坐标，data，是所有的点集，startIdx是下个目标点下标
double calculateTotalDistance(int startIdx, const std::vector<car> &pathPoints)
{
  double totalDistance = 0.0;
  if (startIdx >= pathPoints.size())
  {
    return -1.0; // 或者其他表示错误的特殊值
  }
  const car &fpoint = pathPoints[startIdx];

  double fx = fpoint.x; // 将字符串转换为double
  double fy = fpoint.y; // 将字符串转换为double
  totalDistance += distance_count(lidar_rtk.x, lidar_rtk.y, fx, fy);
  for (size_t i = startIdx; i < pathPoints.size() - 1; ++i)
  {
    const car &point1 = pathPoints[i];
    const car &point2 = pathPoints[i + 1];
    double x1 = point1.x; // 将字符串转换为double
    double y1 = point1.y; // 将字符串转换为double
    double x2 = point2.x; // 将字符串转换为double
    double y2 = point2.y; // 将字符串转换为double
    totalDistance += distance_count(x1, y1, x2, y2);
  }
  return totalDistance;
}

void publishSradsackProgress(int i, const std::vector<car> &pathPoints, int speed, int line_id, int map_id)
{
  rads::rads_acks msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "sradsack_map";
  msg.ack = 3;
  // Calculate progress percentage
  double newpercentage = 100 - (static_cast<double>(pathPoints.size() - i) / pathPoints.size() * 100.0);
  int newbaifenbi = static_cast<int>(newpercentage + 0.5); // rounded percentage
  // Calculate distance and time
  double totalDistance = calculateTotalDistance(i, pathPoints);
  int newlicheng = static_cast<int>(totalDistance);
  double newspeedInMPS = speed / 10.0 / 3.6;
  double newseconds = totalDistance / newspeedInMPS;
  int newduoshaomiao = static_cast<int>(newseconds);
  // Format progress string
  msg.progress = std::to_string(newbaifenbi) + "," +
                 std::to_string(newduoshaomiao) + "," +
                 std::to_string(newlicheng) + "," +
                 std::to_string(line_id) + "," +
                 std::to_string(map_id);
  sradsack_pub_.publish(msg);
}

// 踩点处理
double normalized_heading_diff(double a, double b)
{
  double diff = fabs(a - b);
  return (diff > 180.0) ? 360.0 - diff : diff; // 处理环绕
}

// 计算两点间距离（Haversine公式）
double calculateDistance(double lat1, double lon1, double lat2, double lon2)
{
  const double R = 6371000; // 地球半径，单位：米
  double phi1 = lat1 * M_PI / 180.0;
  double phi2 = lat2 * M_PI / 180.0;
  double delta_phi = (lat2 - lat1) * M_PI / 180.0;
  double delta_lambda = (lon2 - lon1) * M_PI / 180.0;

  double a = sin(delta_phi / 2) * sin(delta_phi / 2) +
             cos(phi1) * cos(phi2) *
                 sin(delta_lambda / 2) * sin(delta_lambda / 2);
  double c = 2 * atan2(sqrt(a), sqrt(1 - a));

  return R * c;
}

int Capture(int img_id)
{
  // 初始化SDK
  NET_DVR_Init();

  // 定义两个摄像头的配置
  std::vector<CameraConfig> cameras = {
      {"192.168.1.64", "admin", "okwy1234", "/home/argo/catkin_ws/src/rads-master/img"},
      {"192.168.1.65", "admin", "ipc12345", "/home/argo/catkin_ws/src/rads-master/reimg"}};

  bool anySuccess = false;

  // 循环处理每个摄像头
  for (const auto &camera : cameras)
  {
    long lUserID = -1;

    // 登录摄像头
    NET_DVR_USER_LOGIN_INFO struLoginInfo = {0};
    NET_DVR_DEVICEINFO_V40 struDeviceInfoV40 = {0};
    struLoginInfo.bUseAsynLogin = false;

    struLoginInfo.wPort = 8000;
    memcpy(struLoginInfo.sDeviceAddress, camera.ip.c_str(), std::min(camera.ip.length(), (size_t)NET_DVR_DEV_ADDRESS_MAX_LEN - 1));
    memcpy(struLoginInfo.sUserName, camera.username.c_str(), std::min(camera.username.length(), (size_t)NAME_LEN - 1));
    memcpy(struLoginInfo.sPassword, camera.password.c_str(), std::min(camera.password.length(), (size_t)NAME_LEN - 1));

    lUserID = NET_DVR_Login_V40(&struLoginInfo, &struDeviceInfoV40);

    if (lUserID < 0)
    {
      printf("摄像头 %s 登录失败, 错误码: %d\n", camera.ip.c_str(), NET_DVR_GetLastError());
      continue; // 继续尝试下一个摄像头
    }

    // 捕获图像
    NET_DVR_JPEGPARA strPicPara = {0};
    strPicPara.wPicQuality = 2;
    strPicPara.wPicSize = 0;
    std::string filepath = camera.saveDir + "/" + std::to_string(img_id) + ".jpeg";
    int iRet = NET_DVR_CaptureJPEGPicture(lUserID, struDeviceInfoV40.struDeviceV30.byStartChan, &strPicPara, const_cast<char *>(filepath.c_str()));

    if (!iRet)
    {
      printf("摄像头 %s 捕获图像失败, 错误码: %d\n", camera.ip.c_str(), NET_DVR_GetLastError());
    }
    else
    {
      printf("摄像头 %s 图像保存成功: %s\n", camera.ip.c_str(), filepath.c_str());
      // 添加修改文件权限的代码
      if (chmod(filepath.c_str(), 0777) == 0)
      {
        printf("成功将文件 %s 权限设置为 777\n", filepath.c_str());
      }
      else
      {
        printf("设置文件 %s 权限失败, 错误码: %d\n", filepath.c_str(), errno);
      }
      anySuccess = true; // 至少有一个摄像头成功
    }

    // 登出摄像头
    NET_DVR_Logout_V30(lUserID);
  }

  // 清理SDK资源
  NET_DVR_Cleanup();

  // 只要有一个摄像头成功捕获图像，就返回成功
  return anySuccess ? HPR_OK : HPR_ERROR;
}

// PID
class AnglePIDController
{
private:
  // PID参数
  double kp, ki, kd;
  double max_output, min_output;

  // 控制器状态
  double integral;
  double prev_error;
  double prev_derivative; // 用于微分项滤波
  ros::Time prev_time;

  // 微分项滤波系数 (0.0~1.0，越小滤波越强)
  const double derivative_filter_alpha = 0.2;

public:
  AnglePIDController(double p, double i, double d, double min, double max)
      : kp(p), ki(i), kd(d), min_output(min), max_output(max),
        integral(0), prev_error(0), prev_derivative(0),
        prev_time(ros::Time::now()) {}

  // 重置控制器状态
  void reset()
  {
    integral = 0;
    prev_error = 0;
    prev_derivative = 0;
    prev_time = ros::Time::now();
    ROS_DEBUG("PID controller reset");
  }

  // 添加getIntegral
  double getIntegral() const
  {
    return integral;
  }

  // 计算PID输出 (主函数)
  double calculate(double setpoint, double pv)
  {
    // === 1. 时间差计算 ===
    ros::Time now = ros::Time::now();
    double dt = (now - prev_time).toSec();
    prev_time = now;
    if (dt <= 0)
      dt = 0.001; // 最小时间步长保护

    // === 2. 角度误差标准化 (-180°~180°) ===
    double error = pv - setpoint; // 关键修改：交换相减顺序
    while (error > 180.0)
      error -= 360.0;
    while (error < -180.0)
      error += 360.0;

    // 增加死区处理，例如当误差在±1度范围内时，误差设为0
    if (error < 1 && error > -1)
    {
      error = 0;
    }

    // === 3. 比例项 ===
    double P = kp * error;

    // === 4. 积分项 (带抗饱和) ===
    integral += error * dt;

    // 计算未限幅的临时输出，用于积分抗饱和判断
    double temp_output = P + (ki * integral) + kd * ((error - prev_error) / dt);

    // 增强积分抗饱和机制：当临时输出达到限幅且误差与输出同方向时，停止积分增长
    bool at_max = (temp_output >= max_output) && (error > 0);
    bool at_min = (temp_output <= min_output) && (error < 0);
    if (at_max || at_min)
    {
      integral -= error * dt; // 撤销刚才的积分增长
    }

    // 积分限幅 (简单条件判断实现)
    double max_integral = (ki != 0) ? (max_output * 0.3 / fabs(ki)) : 0;
    if (integral > max_integral)
    {
      integral = max_integral;
    }
    else if (integral < -max_integral)
    {
      integral = -max_integral;
    }
    double I = ki * integral;

    // === 5. 微分项 (带低通滤波) ===
    double raw_derivative = (error - prev_error) / dt;
    double D = kd * (derivative_filter_alpha * raw_derivative +
                     (1 - derivative_filter_alpha) * prev_derivative);
    prev_derivative = D;
    prev_error = error;

    // === 6. 输出合成与限幅 ===
    double output = P + I + D;
    if (output > max_output)
    {
      output = max_output;
    }
    else if (output < min_output)
    {
      output = min_output;
    }

    ROS_DEBUG_THROTTLE(0.5,
                       "PID Debug: error=%.3f | P=%.3f I=%.3f D=%.3f | output=%.3f",
                       error, P, I, D, output);

    return output;
  }

  // 动态调整PID参数
  void setGains(double p, double i, double d)
  {
    kp = p;
    ki = i;
    kd = d;
    ROS_INFO("PID gains updated: Kp=%.3f Ki=%.3f Kd=%.3f", kp, ki, kd);
  }
};

// 增强版PID (带停车状态管理)
class EnhancedAnglePIDController : public AnglePIDController
{
private:
  const int &stop_car_ref;   // 停车信号引用
  const int &stop_lidar_ref; // 激光雷达停止信号引用
  bool was_stopped;          // 上一次状态

public:
  EnhancedAnglePIDController(double p, double i, double d, double min, double max,
                             const int &stop_car, const int &stop_lidar)
      : AnglePIDController(p, i, d, min, max),
        stop_car_ref(stop_car),
        stop_lidar_ref(stop_lidar),
        was_stopped(false) {}

  double calculate(double setpoint, double pv)
  {
    // === 1. 停车状态检测 ===
    bool is_stopped = (stop_car_ref != 1) || (stop_lidar_ref == 1);

    // === 2. 状态切换处理 ===
    if (is_stopped)
    {
      if (!was_stopped)
      {
        reset(); // 首次进入停车状态时重置控制器
        ROS_WARN("Vehicle stopped! PID reset.");
      }
      was_stopped = true;
      return 0.0; // 强制输出零
    }
    else if (was_stopped)
    {
      reset(); // 从停车恢复时完全重置
      was_stopped = false;
      ROS_WARN("Vehicle resumed from stop.");
    }

    // === 3. 正常PID计算 ===
    double output = AnglePIDController::calculate(setpoint, pv);

    // 在主控制循环中打印积分项值（新增）
    ROS_INFO_THROTTLE(0.5, "Integral raw value: %.3f", getIntegral());
    return output;
  }
};

#endif